/*
 * Link.cpp
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#include <motion/Link.h>

Link::Link() {
	//cout << "Link" << endl;
	length=0;
	mass=0;
	q=0;
	dq=0;
	ddq=0;
	q_old=0;
	dq_old=0;
	ddq_old=0;
	tau=0;
	tipx=0;
	tipy=0;
	tipz=0;
	CGtip_x=0;
	CGtip_y=0;
	CGtip_z=0;
}

Link::~Link() {
	//cout << "\t\t\tLink Deleted" << endl;length;
}

Matrix Link::getCG() const
{
    return CG;
}

double Link::getCGtip_x() const
{
    return CGtip_x;
}

double Link::getCGtip_y() const
{
    return CGtip_y;
}

double Link::getCGtip_z() const
{
    return CGtip_z;
}

Matrix Link::getDH() const
{
    return DH;
}

double Link::getDdq() const
{
    return ddq;
}

double Link::getDdq_old() const
{
    return ddq_old;
}

double Link::getDq() const
{
    return dq;
}

double Link::getDq_old() const
{
    return dq_old;
}

double Link::getLength() const
{
    return length;
}

double Link::getMass() const
{
    return mass;
}

double Link::getOld() const
{
    return q_old;
}

double Link::getQ() const
{
    return q;
}

double Link::getTau() const
{
    return tau;
}

double Link::getTipx() const
{
    return tipx;
}

double Link::getTipy() const
{
    return tipy;
}

double Link::getTipz() const
{
    return tipz;
}

void Link::setCG(Matrix &newCG)
{
	CG.resize(newCG.getX(), newCG.getY());
	CG=newCG;
}

void Link::setCGtip_x(double CGtip_x)
{
    this->CGtip_x = CGtip_x;
}

void Link::setCGtip_y(double CGtip_y)
{
    this->CGtip_y = CGtip_y;
}

void Link::setCGtip_z(double CGtip_z)
{
    this->CGtip_z = CGtip_z;
}

void Link::setDH(Matrix &newDH)
{
	DH.resize(newDH.getX(), newDH.getY());
    DH = newDH;
}

void Link::setDdq(double ddq)
{
    this->ddq = ddq;
}

void Link::setDdq_old(double ddq_old)
{
    this->ddq_old = ddq_old;
}

void Link::setDq(double dq)
{
    this->dq = dq;
}

void Link::setDq_old(double dq_old)
{
    this->dq_old = dq_old;
}

void Link::setLength(double length)
{
    this->length = length;
}

void Link::setMass(double mass)
{
    this->mass = mass;
}

void Link::setOld(double q_old)
{
    this->q_old = q_old;
}

void Link::setQ(double q)
{
    this->q = q;
}

void Link::setTau(double tau)
{
    this->tau = tau;
}

void Link::setTipx(double tipx)
{
    this->tipx = tipx;
}

void Link::setTipy(double tipy)
{
    this->tipy = tipy;
}

void Link::setTipz(double tipz)
{
    this->tipz = tipz;
}

void Link::printInfo(){
	cout << "CG: " << CG;
    cout << "DH: " << DH;
	cout << "Length: " << length << endl;
	cout << "Mass: " << mass << endl;
	cout << "Angle: " << q << endl;
}

Matrix Link::forwardK(){
	Matrix A = DH.transMatrix();
	//cout << "LINK: " << A  << endl;
	return A;
}


